from time import sleep
from havi import ports

# --- Setup ---
servo = ports.servo1()                          # Servo on Servo1 port
ultrasonic = ports.input1("ultrasonic sensor")  # HC-SR04 on Input1

# Sweep parameters
min_angle = 0
max_angle = 180
step = 2# degrees per step (smaller = smoother, but slower)
delay = 0.05 # delay between moves

while True:
    # Sweep left to right
    for angle in range(min_angle, max_angle + 1, step):
        servo.move(angle)
        sleep(delay)  # allow servo to move
        try:
            distance = ultrasonic.distance_cm()
            print(f"{angle},{distance:.2f}")  # angle,distance
        except Exception as e:
            print(f"{angle},OutOfRange")
        sleep(0.1)  # pause before next reading

    # Sweep right to left
    for angle in range(max_angle, min_angle - 1, -step):
        servo.move(angle)
        sleep(delay)
        try:
            distance = ultrasonic.distance_cm()
            print(f"{angle},{distance:.2f}")
        except Exception as e:
            print(f"{angle},OutOfRange")
        sleep(0.1)
